#include "visentech.h"
#include <QDebug>
#include <QTime>
#include "tools.h"
Visentech::Visentech(QObject *parent) : QThread(parent)
{
    stop_Thread = false;
    isStartAcquire = false;
}
Visentech::~Visentech()
{
    stop_Thread = true;
}
void Visentech::run()
{
    int result;
    while (true) {
        if(isStartAcquire&&(Connect_result==0)){
            qDebug()<<"isStartAcquire";
            result = Capture3DData();//采集点云
            qDebug()<<"Capture3DData"<<result;
            if(result)//如果采集点云成功测将点云数据转换成Halcon变量
            {
                GeneratePointMap();
                //GenObjectModel3D();
            }
            isStartAcquire =false;//关闭采集
        }
        if(stop_Thread)
        {
            break;
        }
    }
    VST3D_Exit();//退出VisionTop
    quit();//关闭线程
}
int Visentech::Init()//初始化设备(默认路径安装）
{
    int result = VST3D_Init(); //      缺省安装目录启动，正常窗口启动
    // int result = VST3D_Init(install_path, false);  //缺省安装目录启动,隐藏软件窗口
    if (result != VST3D_RESULT_OK)
    {
        printf("Could not Start Scanner software.\n");
        VST3D_Exit();
        return -1;
    }
    else {
        qDebug()<<"Initresult"<<result;
        return 0;
    }

}
int Visentech::InitPath( const char* install_path)//初始化设备（自定义路径安装）
{
    int result = VST3D_Init(install_path, true);  //缺省安装目录启动,隐藏软件窗口
    if (result != VST3D_RESULT_OK)
    {
        printf("Could not Start Scanner software.\n");
        VST3D_Exit();
        return -1;
    }
    else {
        qDebug()<<"Initresult"<<result;
        return 0;
    }
}
int Visentech::Connect()//连接设备
{
    Connect_result = VST3D_Connect();     // 正常单机扫描，连上扫描仪并进行自检
    if (Connect_result != VST3D_RESULT_OK)
    {
        printf("Could not connect to Scanner.\n");
        VST3D_Exit();
        return -1;
    }
    else{
        qDebug()<<"Connect_result"<<Connect_result;
        return 1;
    }

}
int Visentech::Capture3DData()//获取点云
{
    int result = VST3D_Scan();//  开始多线程单次扫描，设置超时
    if (result != VST3D_RESULT_OK)
    {
        printf("Scan error...\n");
        int nreset = 0;
        while (true)
        {
            Sleep(CAPTURE_TIME_OUT); // 每次重新连接前延时几秒，相机，光栅硬件驱动加载需要时间

            // 单机扫描情况
            result = VST3D_Reset(); // 单机扫描重新连接扫描仪系统，重启

            if (result != VST3D_RESULT_OK)
            {
                printf("Check cables connected to Scanner.\n");  // 重新连接失败，继续尝试
            }
            else
            {
                return 1;;   //重新连接成功，重新扫描
            }
            if (nreset++ > 5)  // 做5次重新连接尝试，如果不能正常连接扫描仪，退出软件系统
            {
                VST3D_Exit();//退出VisionTop
                return 0;

            }
        }
    }
    else {
        return 1;
    }
}
void Visentech::StartAcquire()//采集点云
{
    if(isStartAcquire == false)
    {
        isStartAcquire =true;
    }
}
void Visentech::GenObjectModel3D()//将点云数据转换成halcon变量，通道信号与槽功能的信号发射出去
{
    int totalNum = 0,result;
    VST3D_PT* pPointClouds = nullptr;
    result = VST3D_GetPointClouds(totalNum, &pPointClouds);  // 得到当前单次扫描总点数和乱序的点云
    qDebug()<<"VST3D_GetPointClouds"<<result;
    qDebug()<<"pointer totalNum"<<totalNum;
    HTuple point_x ,point_y,point_z,hv_Length;
    qDebug()<<"start_tiem:"<<QTime::currentTime().toString();
    for (int i = 0; i < totalNum; i++)    // 遍历单次采集到的所有乱序点云，得到点云坐标等信息，通过索引方式[0 1 2 ....]
    {
        VST3D_PT* pt = nullptr;
        VST3D_GetEachPointByIndex(i, &pt);
        double x, y, z;
        x = pt->x;
        y = pt->y;
        z = pt->z;
        point_x[i] = x;
        point_y[i] = y;
        point_z[i] = z;
    }
    qDebug()<<"start_tiem:"<<QTime::currentTime().toString();
    GenObjectModel3dFromPoints(point_x,point_y,point_z,&ObjectModel3D);
    qDebug()<<"start_tiem:"<<QTime::currentTime().toString();
    TupleLength(point_x, &hv_Length);
    if(hv_Length>0)
    {
        emit send_ObjectModel3D(ObjectModel3D);
    }

    qDebug()<<"hv_Length"<<(int)hv_Length;
}
void Visentech::GeneratePointMap() // 生成点云和图像对应关系点云图
{
    unsigned char* pImagL = nullptr;
    unsigned char* pImagR = nullptr;
    int width = 0;
    int height = 0;
    HObject ImageL,ImageR;
    int resultL = VST3D_GetImage(&pImagL, width, height,true);  // 获取单次采集的左相机图片
    if(resultL == 0)
    {
      CmaeraImageProcess(pImagL,width,height,ImageL);
    }
    int resultR = VST3D_GetImage(&pImagR, width, height,false);  // 获取单次采集的右相机图片
    if(resultR == 0)
    {
       CmaeraImageProcess(pImagR,width,height,ImageR,false);
          qDebug()<<"ImageR";
    }
    VST3D_GeneratePointMap(); // 生成点云和图像对应关系点云图
    VST3D_PT* pt = nullptr;
    HTuple point_x ,point_y,point_z,hv_Length;
    int count = 0;
    for (int i = 0; i <  height ; i++)
    {
        for (int j = 0; j < width ; j++)
        {
            VST3D_GetEachPointByPointMap(i, j, width, height, &pt);  // 取出2D图像位置(i,j)对应的点云，以便进行处理
            double x, y, z;
            x = pt->x;
            y = pt->y;
            z = pt->z;
            point_x[count] = x;
            point_y[count] = y;
            point_z[count] = z;
            count++;
        }
    }
    GenObjectModel3dFromPoints(point_x,point_y,point_z,&ObjectModel3D);
    qDebug()<<"start_tiem:"<<QTime::currentTime().toString();
    TupleLength(point_x, &hv_Length);
    if(hv_Length>0)
    {
        emit send_ObjectModel3D(ObjectModel3D);
    }
     qDebug()<<"hv_Length"<<(int)hv_Length;
}
void Visentech::CmaeraImageProcess(unsigned char* pImag,int width,int height,HObject Image,bool lLeft)
{
    int bytewidth = (width * 3 + 3) / 4 * 4; ;
    int bytewidthg = (width + 3) / 4 * 4;
    byte* m_pImageDataR = new byte[bytewidthg * height];
    byte* m_pImageDataG = new byte[bytewidthg * height];
    byte* m_pImageDataB = new byte[bytewidthg * height];
    for (int j = 0; j < height; j++)
    {
        for (int i = 0; i < width; i++)
        {
            m_pImageDataB[j * bytewidthg + i] = pImag[j * bytewidth + i * 3 + 0];
            m_pImageDataG[j * bytewidthg + i] = pImag[j * bytewidth + i * 3 + 1];
            m_pImageDataR[j * bytewidthg + i] = pImag[j * bytewidth + i * 3 + 2];

        }
    }

    GenImage3(&Image,(HTuple)"byte",(HTuple)width,(HTuple)height,(Hlong)m_pImageDataR,(Hlong)m_pImageDataG,(Hlong)m_pImageDataB);
    if(lLeft)
    {
        emit send_ImageL(Image);
    }
    else {
        emit send_ImageR(Image);
    }
}
void Visentech::SaveColudPoints(QString filename)//保存点云
{
    WriteObjectModel3d(ObjectModel3D, "om3", (HTuple)filename.toStdString().c_str(), HTuple(), HTuple());
}
void Visentech::SaveImage(const char* filename,bool lLeft)//保存图像
{
    unsigned char* pImagL = nullptr;
    unsigned char* pImagR = nullptr;
    int width = 0;
    int height = 0;
    HObject ImageL,ImageR;
    if(lLeft)
    {
        int resultL = VST3D_GetImage(&pImagL, width, height);  // 获取单次采集的左相机图片
        if(resultL==0)
        {
            Write24BitImg2BmpFile(pImagL,width,height,filename);
             qDebug()<<filename;
        }
    }
    else {
        int resultR = VST3D_GetImage(&pImagR, width, height,false);  // 获取单次采集的右相机图片
        if(resultR==0)
        {
            Write24BitImg2BmpFile(pImagR,width,height,filename);
            qDebug()<<filename;
        }
    }
}
void Visentech::Uninit()//关闭设备
{
    stop_Thread = true;
    VST3Ds_Exit();
}
